www.gusucode.com > VC 3D弹道仿真程序源码文件-源码程序 > VC 3D弹道仿真程序源码文件-源码程序/code/GuidLaw.cpp

    #include "stdafx.h"
//Download by http://www.NewXing.com
#include "GuidLaw.h"
#include "Atmosphere.h"
#include "AeroForce.h"
#include "EarthModel.h"
#include "3DMODEL.h"

const double PI=3.1415926535 ;
const double RAD=180./PI ;

extern double now_time, t_start, t_start_1, t_start_2 ;
extern double S_ref ;

CGuidance::CGuidance( )
{
}

CGuidance::~CGuidance()
{
}
CAtmosphere *pAtmosphere_G = new CAtmosphere ;
CAeroForce  *pAeroForce_G =  new CAeroForce;
CEarth      *pEarth_G     =  new CEarth ; 

double  G, gr, g_we;

void CGuidance::Cal_Guidance( double XS[35], double& nyc, double& nzc )
{
	double cita_, Psi_, Ky, Kz ;
	G = pAtmosphere_G->Get_g_precision(XS[22], XS[17], gr, g_we );

	 if ( (XS[3]>50000) && (XS[15] < 6000.0 ) )  //真实目标
	{
    	Ky =4; 		
		cita_ = Ky * XS[25] + 0.04 * ( XS[24] - (-80)/RAD );		
	}
	else 
	{
		Ky =2; 
		cita_ = Ky * XS[25];
	}															//	XS[24] = q[0] ;	XS[25] = q[1] ;	

	if ( (XS[3]>50000) && (XS[15] < 15000.0 ) )  //真实目标
	{
    	Kz =4; 		
		Psi_ = Kz * XS[27] + 0.03 * ( XS[26] - (0)/RAD );
	}
	else 
	{
		Kz =2;
		Psi_ = Kz * XS[27] + 0.03 * ( XS[26] - (0)/RAD );
	}															//	XS[26] = q[2] ;	XS[27] = q[3] ;

	nyc = cita_ * XS[9]/G + cos( XS[10] );
	nzc = -Psi_ * XS[9]/G ;

	return ;
}

void CGuidance::Control( double XS[35] )
{
	double q,rho,Ma,Nyc,Nzc,Alpha,Beta;
	double m=XS[6];

	G = pAtmosphere_G->Get_g_precision(XS[22], XS[17], gr, g_we );
	rho = pAtmosphere_G->Get_rho_Airforce( XS[15] );
	q=0.5*rho*XS[9]*XS[9];
	Ma = pAtmosphere_G->GetMa_Airforce( XS[9],XS[15] );
	pAeroForce_G->Cal_Cxyz( );
	Cal_Guidance( XS, Nyc, Nzc );

	Alpha=0;	Beta=0;

	if ( ( XS[3]<40000 )&&( XS[15] <15000 ) &&  (now_time > 6.3)   ) //6.3s之前为主动段 
	{												    	//6.3s之后为虚拟比例导引段（15km高度）
		cout<<"虚拟比例导引 1"<<endl;

        Alpha = m*G * Nyc/( q*S_ref*pAeroForce_G->Cy_alpha )/RAD;
		Beta =  m*G * Nzc/( q*S_ref*pAeroForce_G->Cz_beta )/RAD;

        t_start = now_time;
		t_start_1 = now_time;
		t_start_2 = now_time;
		
		XS[13] = Alpha;							// 13 攻角
		XS[14] = Beta;							// 14 侧滑beta 
	}

	if ( ( XS[15] >15000 ) && ( t_start+0.01 ==now_time ) )		//15km之后为无控段
	{	
		cout<<"		  无控段"<<endl;

		Alpha=0.0;
		Beta=0;

        t_start = now_time;
		t_start_1 = now_time;
		t_start_2 = now_time;
		
		XS[13] = Alpha;							// 13 攻角
		XS[14] = Beta;							// 14 侧滑beta 
	}
   
	if ( ( XS[15] <15000 )&&( XS[15] >6000 ) && ( t_start_1+0.01 == now_time )  )		//从15km下降到6km为拉起段
	{
		cout<<"				   拉起段"<<endl;

	    Alpha= 5/RAD;
		Beta=m*G *Nzc/( q*S_ref*pAeroForce_G->Cz_beta )/RAD;

		t_start_1= now_time;
		t_start_2= now_time;
		
		XS[13] = Alpha;							// 13 攻角
		XS[14] = Beta;							// 14 侧滑beta 
	}	

	if ( ( XS[3]> 50000 )&&( XS[15] <6000 )  && ( t_start_2 < now_time )  )		//下降到6km之后为真实比例导引段
	{
	    cout<<"						真实比例导引 2 "<<endl;
		   		   
		Alpha = m*G * Nyc/( q*S_ref*pAeroForce_G->Cy_alpha )/RAD;
		Beta = m*G * Nzc/( q*S_ref*pAeroForce_G->Cz_beta )/RAD;
		if ( Alpha < -8/RAD )	//限幅
		Alpha = -8/RAD;
		 
		XS[13] = Alpha;							// 13 攻角
		XS[14] = Beta;							// 14 侧滑beta 
	}

	return ;
}

void CGuidance::Cal_Overload( double XS[35], double dXS[7] )
{
	double Nxc_o, Nyc_o, Nzc_o;			// 过载在发射坐标系中的投影launch
	double Nxc_2, Nyc_2;				// 过载在弹道坐标系中的投影
	double R0x, R0y, R0z;
	C3DModel * p3DModel_G = new C3DModel;

	G = pAtmosphere_G->Get_g_precision(XS[22], XS[17], gr, g_we );
	pEarth_G->Cal_R0_xyz( p3DModel_G->B0, p3DModel_G->A0, R0x, R0y, R0z ) ;
	
	Nxc_o = ( dXS[0] - gr * (XS[3] + R0x)/XS[22] - g_we * cos(XS[17]) * cos(p3DModel_G->A0) )/ G ;
	Nyc_o = ( dXS[1] - gr * (XS[4] + R0y)/XS[22] - g_we * sin(XS[17]) )/ G ;
	Nzc_o = ( dXS[2] - gr * (XS[5] + R0z)/XS[22] )/ G ;

	Nxc_2 = (  Nxc_o*( cos(XS[10])*cos(XS[11]) ) + Nyc_o * sin(XS[10]) + Nzc_o * ( -cos(XS[10])*sin(XS[11]) ) ) ;
	Nyc_2 = (  Nxc_o*( -sin(XS[10])*cos(XS[11]) ) + Nyc_o * cos(XS[10]) + Nzc_o * sin(XS[10])*sin(XS[11]) ) ;
	XS[33] = Nxc_2 ; 
	XS[34] = Nyc_2 ;

	return ;
}